#pragma once
#ifndef bsp_math_hpp
#define bsp_math_hpp

#include <math.h>

//#define PI 3.141592653589793f

#define vfloat float

typedef vfloat VECTOR3D[3];
typedef vfloat EQUATION[4];
typedef vfloat TEXCOORD[2];

#define ABS(x) ((x)>=0?(x):-(x))

#define VECTOR_CROSS(vector,v1,v2)\
 vector[0]= v1[1]*v2[2]-v1[2]*v2[1];\
 vector[1]= v1[2]*v2[0]-v1[0]*v2[2];\
 vector[2]= v1[0]*v2[1]-v1[1]*v2[0];

#define VECTOR_DOT(v1,v2)\
 (v1[0]*v2[0]+v1[1]*v2[1]+v1[2]*v2[2])

#define VECTOR_SUB(vector,v1,v2)\
 vector[0]= v1[0]-v2[0];\
 vector[1]= v1[1]-v2[1];\
 vector[2]= v1[2]-v2[2];

#define VECTOR_NORM(norm,v0,v1,v2)\
 norm[0]= (v1[1]-v0[1])*(v2[2]-v0[2])-(v1[2]-v0[2]*v2[1]-v0[1]);\
 norm[1]= (v1[2]-v0[2])*(v2[0]-v0[0])-(v1[0]-v0[0]*v2[2]-v0[2]);\
 norm[2]= (v1[0]-v0[0])*(v2[1]-v0[1])-(v1[1]-v0[1]*v2[0]-v0[0]);

#define VECTOR_LENGTH(vector)\
 (sqrt((vector[0]*vector[0]+vector[1]*vector[1]+vector[2]*vector[2])))

#define VECTOR_MUL_SCALAR(vector,scalar)\
 vector[0]*= scalar;\
 vector[1]*= scalar;\
 vector[2]*= scalar;

#define VECTOR_MUL_SCALAR_2D(vector,scalar)\
 vector[0]*= scalar;\
 vector[1]*= scalar;

#define VECTOR_DIV_SCALAR(vector,scalar)\
 vector[0]/= scalar;\
 vector[1]/= scalar;\
 vector[2]/= scalar;

#define VECTOR_ADD(vector,v1,v2)\
 vector[0]= v1[0]+v2[0];\
 vector[1]= v1[1]+v2[1];\
 vector[2]= v1[2]+v2[2];

#define VECTOR_ADD_2D(vector,v1,v2)\
 vector[0]= v1[0]+v2[0];\
 vector[1]= v1[1]+v2[1];

#define VECTOR_INC(vector,v)\
 vector[0]+= v[0];\
 vector[1]+= v[1];\
 vector[2]+= v[2];

#define VECTOR_DEC(vector,v)\
 vector[0]-= v[0];\
 vector[1]-= v[1];\
 vector[2]-= v[2];

#define PLANE_VECTORS(orig,norm,eq,v_orig,v_norm)\
 orig[0]= v_orig[0];\
 orig[1]= v_orig[1];\
 orig[2]= v_orig[2];\
 norm[0]= v_norm[0];\
 norm[1]= v_norm[1];\
 norm[2]= v_norm[2];\
 eq[0]= norm[0];\
 eq[1]= norm[1];\
 eq[2]= norm[2];\
 eq[3]= -(norm[0]*orig[0]+norm[1]*orig[1]+norm[2]*orig[2]);

#define PLANE_VECTORS_NORMALIZE(orig,norm,eq,v_orig,v_norm)\
 orig[0]= v_orig[0];\
 orig[1]= v_orig[1];\
 orig[2]= v_orig[2];\
 norm[0]= v_norm[0];\
 norm[1]= v_norm[1];\
 norm[2]= v_norm[2];\
 eq[3]= VECTOR_LENGTH(norm);\
 VECTOR_DIV_SCALAR(norm,eq[3])\
 eq[0]= norm[0];\
 eq[1]= norm[1];\
 eq[2]= norm[2];\
 eq[3]= -(norm[0]*orig[0]+norm[1]*orig[1]+norm[2]*orig[2]);

#define PLANE_TRIANGLE(orig,norm,eq,v0,v1,v2)\
 VECTOR_SUB(orig,v1,v0);\
 VECTOR_SUB(eq,v2,v0);\
 VECTOR_CROSS(norm,orig,eq);\
 eq[3]= VECTOR_LENGTH(norm);\
 VECTOR_DIV_SCALAR(norm,eq[3])\
 orig[0]= v0[0];\
 orig[1]= v0[1];\
 orig[2]= v0[2];\
 eq[0]= norm[0];\
 eq[1]= norm[1];\
 eq[2]= norm[2];\
 eq[3]= -(norm[0]*orig[0]+norm[1]*orig[1]+norm[2]*orig[2]);

#define VECTOR_COPY(vector,v)\
 vector[0]= v[0];\
 vector[1]= v[1];\
 vector[2]= v[2];

#define VECTOR_COPY_2D(vector,v)\
 vector[0]= v[0];\
 vector[1]= v[1];

#define VECTOR_DIV(vector,v0,v1)\
 vector[0]= v0[0]/v1[0];\
 vector[1]= v0[1]/v1[1];\
 vector[2]= v0[2]/v1[2];

#define VECTOR_MUL(vector,v0,v1)\
 vector[0]= v0[0]*v1[0];\
 vector[1]= v0[1]*v1[1];\
 vector[2]= v0[2]*v1[2];

#define PLANE_FRONT_FACING_VECTOR(norm,dir)\
 (VECTOR_DOT(norm,dir)<= 0)

#define PLANE_SIGNED_DIST_POINT(norm,eq,p)\
 ((VECTOR_DOT(p,norm))+eq[3])

inline void normalize(VECTOR3D vector);

inline void set_length(VECTOR3D vector, VECTOR3D v, vfloat l);

#endif

